/**
 * This Module is designed to manage wheel motor speed.
 * It uses Locomotion and Odometry to compute speed and position of the robot.
 *    
 * @author Jerome Monceaux
 *
 */
package org.hypheli.testApps;

import java.util.ArrayList;

import ioio.lib.api.IOIO;
import ioio.lib.api.TwiMaster;
import ioio.lib.api.exception.ConnectionLostException;

import org.hh.common.HHObject;
import org.hh.common.KeyPoint;
import org.hh.locodo.GenericVirtualMotor;
import org.hh.locodo.IoioMotorControler;
//import org.hh.locodo.IoioMotorControlerWithoutEncoder;

import com.googlecode.javacv.cpp.opencv_core.IplImage;

import android.content.Context;
import android.graphics.Canvas;
import android.graphics.Color;
import android.graphics.Paint;
import android.util.Log;
import android.view.MotionEvent;

public class ModLoco extends HHObject {

	// Reference to the ioio class instance
	private IOIO fIoio;
	private boolean fIoioInit = false;
	
	// MotorControler that send pwm but do not read encoder value 
	private IoioMotorControler fMotorLeft;
	private IoioMotorControler fMotorRight;
	
	// Generic motor without hardware connection
	private GenericVirtualMotor fMotorVirtualLeft;
	private GenericVirtualMotor fMotorVirtualRight;
	
	// Locomotion Engine to compute pid and field position 
	private Mobile fLocoEngine;
	
	
	// Still need to clean robot and field space 
	int fOffsetX = 0;
	int fOffsetY = 0;

	// Conversion 
	double fConvPixToMeter = 1.0 / 250.0;
  double fConvMeterToPix = 1.0 / fConvPixToMeter;
  
  // Geometric robot description
  double fWheelDistanceToCenter = 0.14;
  double fWheelDiameter = 0.05;
  double fEncoderWheelPerimeter = Math.PI * fWheelDiameter;
  int fEncoderTickPerRotation = 1024;
  
  // store last encoder value to compute real speed
  int fLastLeftEncoderValue = 0;
  int fLastRightEncoderValue = 0;
  
  // plot the real speed curve
  int fCurrCurvePosition = 0;
  float [] fCurveLeftPoints;
  float [] fCurveRightPoints;
  
  boolean fPlotSpeed = true;
  
  TwiMaster fTwi;
  
	public ModLoco(String name, double startposx, double startposy, double startposa, 
			double encoderWheelPerimeter,
			double encoderWheelsDistanceToCenter, int encoderTickPerRotation, double convMeterToPix) {
		super(name);
		fConvMeterToPix = convMeterToPix;
		fConvPixToMeter = 1./fConvMeterToPix;
		fEncoderWheelPerimeter = encoderWheelPerimeter;
		fWheelDiameter = fEncoderWheelPerimeter / Math.PI;
		fWheelDistanceToCenter = encoderWheelsDistanceToCenter;
		fEncoderTickPerRotation = encoderTickPerRotation;

		ArrayList<KeyPoint> shape = new ArrayList<KeyPoint>();
		shape.add(new KeyPoint(-0.15, -0.15));
		shape.add(new KeyPoint(-0.10, -0.13));
		shape.add(new KeyPoint( 0.10, -0.13));
		shape.add(new KeyPoint( 0.15, -0.15));
		shape.add(new KeyPoint( 0.13, -0.10));
		shape.add(new KeyPoint( 0.13,  0.10));
		shape.add(new KeyPoint( 0.15,  0.15));
		shape.add(new KeyPoint( 0.10,  0.13));
		shape.add(new KeyPoint(-0.10,  0.13));
		shape.add(new KeyPoint(-0.15,  0.15));
		shape.add(new KeyPoint(-0.13,  0.10));
		shape.add(new KeyPoint(-0.13, -0.10));
		
		fLocoEngine = new Mobile("", shape,fWheelDiameter, fWheelDistanceToCenter, fEncoderTickPerRotation, fConvMeterToPix);
		KeyPoint startpos = new KeyPoint();
		startpos.x = startposx;
		startpos.y = startposx;
		startpos.a = startposa;
		fLocoEngine.setPosition(startpos);
	}

	protected void finalize() throws Throwable
	{
		fMotorLeft.close();
		fMotorRight.close();
		if( fTwi != null)
			fTwi.close();
	  super.finalize(); //not necessary if extending Object.
	} 
	
	/**
	 * return Locomotion instance
	 */
	Mobile getMobile(){
		return fLocoEngine;
	}
	/**
	 * Inherit from HHmodule, call systematically even if the hardware is not connected
	 */
	public void initIOIO(boolean isSimulation, IOIO ioio){

		fIoio = ioio;

		if( isSimulation ){
			fMotorVirtualLeft = new GenericVirtualMotor();
			fMotorVirtualRight = new GenericVirtualMotor();
			fLocoEngine.setMotors(fMotorVirtualLeft, fMotorVirtualRight);			
		}else{
			try {
				fTwi = ioio.openTwiMaster(1 /*fI2CIoioNumber*/, TwiMaster.Rate.RATE_400KHz, true);
				fMotorLeft = new IoioMotorControler(
																							10 		/* PwmPin number*/, 
																							11 		/* SignPin number*/, 
																							0x18 	/*i2CDeviceAdress*/, fIoio, fTwi);
				fMotorRight = new IoioMotorControler(
																							12 		/* PwmPin number*/,
																							13 		/* SignPin number*/, 
																							0x05 	/*i2CDeviceAdress*/, fIoio, fTwi);
			} catch (ConnectionLostException e) {
				Log.e("Mobile", e.toString());
			}
			fLocoEngine.setMotors(fMotorLeft, fMotorRight);
		}
		fLocoEngine.setMaxSpeed( 1. );
		fIoioInit = true;
	}
	

	/**
	 * Inherit from HHmodule, call systematically even if the hardware is not connected
	 */
	public void updateIOIO(){
		if(fLocoEngine == null) return;
		try {
			fLocoEngine.update();
		} catch (ConnectionLostException e) {
			Log.e("Mobile", e.toString());
		}
		setChanged();
		notifyObservers(new Object[]{ "currentPosition", new Object[]{fLocoEngine.getPosition()}});
	}

	/**
	 * set the real speed plot visible or not
	 * @param onoff
	 */
  public void setSpeedPlotVisible( boolean onoff){
  	fPlotSpeed = onoff;
  }
  
	/**
	 * get the real speed plot visible
	 * @param onoff
	 */
  public boolean getSpeedPlotVisible(){
  	return fPlotSpeed;
  }
  
  /**
   * Inherit from HHModule. Call on view initialisation.
   */
  public void onInitView(Context context){
	  fCurveLeftPoints = new float[200];
	  fCurveRightPoints = new float[200];
  }
  
  public double getAngleP()
  {
  	return fLocoEngine.getAngleP();
  }
  
  public void setAnglePid(double P, double I, double D)
  {
  	fLocoEngine.setAngleP(P);
  	fLocoEngine.setAngleI(I);
  	fLocoEngine.setAngleD(D);
  }

  public void setDistancePid(double P, double I, double D)
  {
  	fLocoEngine.setDistanceP(P);
  	fLocoEngine.setDistanceI(I);
  	fLocoEngine.setDistanceD(D);
  }

	public void setTarget(double x, double y, double angle){
		fLocoEngine.setTarget( new KeyPoint( x,y,angle,0) );
  }
  
	public Object[] getPosition(){
		KeyPoint p = fLocoEngine.getPosition();
		Object [] res = new Object[3];
		res[0] = p.x;
		res[1] = p.y;
		res[2] = p.a;
		return res;
  }
  

  /**
   * Inherit from HHModule. Call by the onTouch callback of the view.
   */
	public boolean onTouch(MotionEvent event){
		if(fLocoEngine == null) return true;
		float touchX = (int) event.getX();
    float touchY = (int) event.getY();
    KeyPoint pos = fLocoEngine.getPosition();

    Log.i("RobotView", "old pos: " + Double.toString(pos.x) +" , "+ Double.toString(pos.y));
    Log.i("RobotView", "new pos: " +  Float.toString(touchX) +" , "+ Float.toString(touchY));
    
    fLocoEngine.setTarget(new KeyPoint(
							touchX * fConvPixToMeter, 
							touchY * fConvPixToMeter
					));

		return true;
  }

	public void drawNoObstacleShape(Canvas canvas, Paint paint, IplImage probaImg, int zoomFactor){
  	if(fLocoEngine == null) return;
  	fLocoEngine.drawNoObstacleShape(canvas, paint, probaImg, zoomFactor);
	}


  /**
   * Inherit from HHModule. Call to refresh the draw.
   */
	public void onDraw(Canvas canvas, Paint robotPaint){
		String statusString = "ModLoco ok";
		robotPaint.setColor(Color.GREEN);
  	if( !fIoioInit ){
  		robotPaint.setColor(Color.RED);
  		statusString = "ModLoco not init ";
  	}else{
    	if(fLocoEngine == null){
    		robotPaint.setColor(Color.GRAY);
    		statusString = "ModLoco engine nok";
    	}
  	}

		final int connectedPosY = 473 - 10; // position in the screen
		final int connectedPosX = 685;
  	canvas.drawCircle(connectedPosX, connectedPosY, 3, robotPaint);
  	canvas.drawText(statusString, connectedPosX+10, connectedPosY+5, robotPaint);

		if(fLocoEngine == null || !fIoioInit) return;
  	fLocoEngine.onDraw(canvas, robotPaint);
	}
}
